//
// Created by usl on 3/5/21.
//


#define PCL_NO_PRECOMPILE // !! BEFORE ANY PCL INCLUDE!!

#include "ros/ros.h"
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseStamped.h>

#include "utils/pcl_utils.h"

#include <iostream>
#include <vector>
#include <string>
#include <algorithm>
#include <filesystem>
#include "track/lidarOdometry.h"

namespace fs = std::filesystem;

int main(int argc, char** argv) {
    /// Launch ros node
    ros::init(argc, argv, "ros_pair_lodom_imu");
    ros::NodeHandle nh("~");

    ros::Publisher imu_publisher = nh.advertise<sensor_msgs::Imu>("/imu", 1);
    ros::Publisher lidar_publisher = nh.advertise<sensor_msgs::Image>("/rslidar_points", 1);
    ros::Publisher image_publisher = nh.advertise<sensor_msgs::Image>("/image_raw", 1);
    ros::Publisher pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("/lidar_odometry", 1);
    ros::Publisher path_publisher = nh.advertise<nav_msgs::Path>("/trajectory", 1);

    /// Our topics (IMU and LIDAR)
    std::string topic_imu;
    std::string topic_lidar;
    std::string topic_image;
    double ndt_resolution;
    std::string lo_trajectory_filename;
    std::string imu_data_filename;
    nh.param<std::string>("topic_imu", topic_imu, "/imu");
    nh.param<std::string>("topic_lidar", topic_lidar, "/lidar");
    nh.param<std::string>("topic_image", topic_image, "/miivii_gmsl/image1");
    nh.param<double>("ndt_resolution", ndt_resolution, 0.5);
    nh.param<std::string>("lo_trajectory_filename", lo_trajectory_filename, "/home/usl/catkin_ws/src/linkalibr/data/lo_trajectory.csv");

    ROS_INFO_STREAM("topic_imu: " << topic_imu);
    ROS_INFO_STREAM("topic_lidar: " << topic_lidar);
    ROS_INFO_STREAM("ndt_resolution: " << ndt_resolution);
    ROS_INFO_STREAM("lo_trajectory_filename: " << lo_trajectory_filename);

    /// Get our start location and how much of the bag we want to play
    /// Make the bag duration < 0 to just process to the end of the bag
    double bag_start, bag_durr;
    nh.param<double>("bag_start", bag_start, 0);
    nh.param<double>("bag_durr", bag_durr, -1);
    ROS_INFO_STREAM("bag start: " << bag_start);
    ROS_INFO_STREAM("bag duration: " << bag_durr);

    /// Location of the ROS bag we want to read in
    std::string path_to_bag;
    nh.param<std::string>("path_bag", path_to_bag, "/home/usl/datasets/");
    ROS_INFO_STREAM("ROS BAG PATH is: " << path_to_bag.c_str());
    std::vector<std::string> paths;

    paths.push_back("/home/steve/Documents/Datasets/subaru/subaru_2024-09-06-14-31-21_0.bag");
    paths.push_back("/home/steve/Documents/Datasets/subaru/subaru_2024-09-06-14-32-22_1.bag");
    paths.push_back("/home/steve/Documents/Datasets/subaru/subaru_2024-09-06-14-33-23_2.bag");
    paths.push_back("/home/steve/Documents/Datasets/subaru/subaru_2024-09-06-14-34-36_3.bag");
    paths.push_back("/home/steve/Documents/Datasets/subaru/subaru_2024-09-06-14-35-44_4.bag");
    // try {
    //     // 检查目录是否存在
    //     if (fs::exists(path_to_bag) && fs::is_directory(path_to_bag)) {
    //         // 迭代目录中的所有文件
    //         for (const auto& entry : fs::directory_iterator(path_to_bag)) {
    //             // 检查文件扩展名是否为 .bag
    //             if (entry.path().extension() == ".bag") {
    //                 paths.push_back(entry.path().string());
    //                 std::cout << "Found rosbag: " << entry.path().string() << std::endl;
    //             }
    //         }
    //     } else {
    //         std::cerr << "Directory does not exist or is not a directory: " << path_to_bag << std::endl;
    //         return 1;
    //     }
    // } catch (const fs::filesystem_error& e) {
    //     std::cerr << "Filesystem error: " << e.what() << std::endl;
    //     return 1;
    // } catch (const std::exception& e) {
    //     std::cerr << "Error: " << e.what() << std::endl;
    //     return 1;
    // }

    /// Lidar Odometry object (Tracker)
    lin_core::LidarOdometry::Ptr LOdom;
    /// Initialize Lidar Odometry object
    LOdom = std::make_shared<lin_core::LidarOdometry>(ndt_resolution, "", true);
    
    ros::Time current_time = ros::Time::now();
    nav_msgs::Path esti_path;
    esti_path.header.stamp = current_time;
    esti_path.header.frame_id = "odom";

    std::ofstream imu_file_writer;
    imu_file_writer.open(imu_data_filename);
    std::ofstream lodom_file_write;
    lodom_file_write.open(lo_trajectory_filename);

    for (const auto& path : paths) {

        /// Load rosbag here, and find messages we can play
        rosbag::Bag bag;
        bag.open(path, rosbag::bagmode::Read);

        /// We should load the bag as a view
        /// Here we go from beginning of the bag to the end of the bag
        rosbag::View view_full;
        rosbag::View view;

        /// Start a few seconds in from the full view time
        /// If we have a negative duration then use the full bag length
        view_full.addQuery(bag);
        ros::Time time_init = view_full.getBeginTime();
        time_init += ros::Duration(bag_start);
        ros::Time time_finish =
                (bag_durr < 0) ? view_full.getEndTime() : time_init + ros::Duration(bag_durr);
        
        ROS_INFO_STREAM("Rosbag = " << path);        
        ROS_INFO_STREAM("Time start = " << std::fixed << std::setprecision(9) << time_init.toSec());
        ROS_INFO_STREAM("Time end = " << std::fixed << std::setprecision(9) << time_finish.toSec());
        
        view.addQuery(bag, time_init, time_finish);
        /// Check to make sure we have data to play
        if (view.size() == 0) {
            ROS_ERROR_STREAM("No messages to play on specified topics.  Exiting.");
            ros::shutdown();
            return EXIT_FAILURE;
        }

        for (const rosbag::MessageInstance& m : view) {
            /// If ROS wants us to stop, break out
            if(!ros::ok())
                break;
            /// Handle IMU measurement
            sensor_msgs::Imu::ConstPtr s_imu = m.instantiate<sensor_msgs::Imu>();
            if (s_imu != nullptr && m.getTopic() == topic_imu) {
                imu_publisher.publish(*s_imu);
            }

            /// Handle IMAGE measurement
            sensor_msgs::Image::ConstPtr s_img = m.instantiate<sensor_msgs::Image>();
            if (s_img != nullptr && m.getTopic() == topic_image) {
                image_publisher.publish(*s_img);
            }

            /// Handle Lidar measurement
            sensor_msgs::PointCloud2::ConstPtr s_lidar = m.instantiate<sensor_msgs::PointCloud2>();
            if (s_lidar != nullptr && m.getTopic() == topic_lidar) {                
                lin_core::VPointCloud::Ptr cloud_pcl(new lin_core::VPointCloud);
                pcl::fromROSMsg(*s_lidar, *cloud_pcl);
                LOdom->feedScan((*s_lidar).header.stamp.toSec(), cloud_pcl);
                LOdom->append_and_update(true);
                Eigen::Vector3d L0_t_Lk = LOdom->get_current_odometry().pose.block(0, 3, 3, 1);
                Eigen::Matrix3d L0_R_Lk = LOdom->get_current_odometry().pose.block(0, 0, 3, 3);
                Eigen::Quaterniond L0_quat_Lk = Eigen::Quaterniond(L0_R_Lk);
                geometry_msgs::PoseStamped pose_msg;
                pose_msg.header.stamp = s_lidar->header.stamp;
                pose_msg.header.frame_id = s_lidar->header.frame_id;
                pose_msg.pose.position.x = L0_t_Lk.x();
                pose_msg.pose.position.y = L0_t_Lk.y();
                pose_msg.pose.position.z = L0_t_Lk.z();
                pose_msg.pose.orientation.x = L0_quat_Lk.x();
                pose_msg.pose.orientation.y = L0_quat_Lk.y();
                pose_msg.pose.orientation.z = L0_quat_Lk.z();
                pose_msg.pose.orientation.w = L0_quat_Lk.w();
                pose_publisher.publish(pose_msg);
                esti_path.poses.push_back(pose_msg);
                path_publisher.publish(esti_path);
                lidar_publisher.publish(*s_lidar);
                Eigen::AngleAxisd angleAxis(L0_R_Lk);
                lodom_file_write << (*s_lidar).header.stamp.toNSec() << "," << L0_quat_Lk.x() << "," << L0_quat_Lk.y() << ","<< L0_quat_Lk.z() << ","<< L0_quat_Lk.w()
                                << "," << L0_t_Lk.x() << ","<< L0_t_Lk.y() << ","<< L0_t_Lk.z() << "\n";
            }
        }
    }
    return EXIT_SUCCESS;
}